import numpy as np
import math
import pandas as pd
import time
from copy import deepcopy
import os

# -------------------------- 1. 基础参数与复用函数 --------------------------
# 目标与导弹参数
target_true = np.array([0, 200, 0])  # 真目标（圆柱）底面圆心
r, h = 7, 10  # 真目标半径7m，高10m
target_fake = np.array([0, 0, 0])  # 假目标（原点）
missile_M1 = np.array([20000, 0, 2000])  # M1导弹初始位置
missile_speed = 300  # 导弹速度300m/s

# 无人机参数
drone_FY1 = np.array([17800, 0, 1800])  # FY1初始位置
drone_speed_range = [70, 140]  # 速度约束70~140m/s

# 烟幕参数
g = 9.8  # 重力加速度
smoke_radius = 10  # 有效半径10m
smoke_sink_speed = 3  # 下沉速度3m/s
smoke_valid_time = 20  # 有效时长20s
NUM_SMOKE = 3  # 烟幕弹数量

# 导弹到达假目标时间（核心约束）
missile_distance_to_fake = np.linalg.norm(target_fake - missile_M1)
missile_time_to_fake = missile_distance_to_fake / missile_speed  # ≈66.999s
print(f"导弹到达假目标时间：{missile_time_to_fake:.3f}s（烟幕起爆需早于该时间）")


def get_cylinder_bounding_box_vertices(target_center, radius, height):
    """计算真目标外接长方体顶点（用于遮蔽判断）"""
    x_min, x_max = target_center[0] - radius, target_center[0] + radius
    y_min, y_max = target_center[1] - radius, target_center[1] + radius
    z_min, z_max = target_center[2], target_center[2] + height
    return [
        np.array([x_min, y_min, z_min]), np.array([x_max, y_min, z_min]),
        np.array([x_max, y_max, z_min]), np.array([x_min, y_max, z_min]),
        np.array([x_min, y_min, z_max]), np.array([x_max, y_min, z_max]),
        np.array([x_max, y_max, z_max]), np.array([x_min, y_max, z_max])
    ]


target_vertices = get_cylinder_bounding_box_vertices(target_true, r, h)  # 预计算顶点


def missile_real_time_position(missile_init_pos, t):
    """计算导弹在时间t的位置"""
    if t >= missile_time_to_fake - 1e-6:
        return target_fake
    if t < 0:
        return missile_init_pos
    dir_vec = target_fake - missile_init_pos
    dir_norm = np.linalg.norm(dir_vec)
    return missile_init_pos + (dir_vec / dir_norm) * missile_speed * t if dir_norm > 1e-6 else missile_init_pos


def get_smoke_explosion_center(drone_init_pos, v, theta, t_drop, t_detonate):
    """计算烟幕弹起爆中心点"""
    # 投放点位置
    drop_x = drone_init_pos[0] + v * math.cos(theta) * t_drop
    drop_y = drone_init_pos[1] + v * math.sin(theta) * t_drop
    drop_z = drone_init_pos[2]
    # 起爆点位置（平抛+自由下落）
    explosion_x = drop_x + v * math.cos(theta) * t_detonate
    explosion_y = drop_y + v * math.sin(theta) * t_detonate
    fall_distance = 0.5 * g * (t_detonate ** 2)
    explosion_z = max(drop_z - fall_distance, 0.0)  # 不低于地面
    return np.array([explosion_x, explosion_y, explosion_z])


def smoke_real_time_center(drone_init_pos, v, theta, t_drop, t_detonate, t):
    """计算时间t时的烟幕中心位置"""
    detonate_t = t_drop + t_detonate
    if t < detonate_t - 1e-6:
        return np.array([0, 0, 0])  # 未起爆，无效
    explosion_center = get_smoke_explosion_center(drone_init_pos, v, theta, t_drop, t_detonate)
    sink_distance = smoke_sink_speed * (t - detonate_t)
    return np.array([explosion_center[0], explosion_center[1], max(explosion_center[2] - sink_distance, 0.0)])


def is_segment_sphere_intersect(missile_pos, vertex_pos, smoke_center, smoke_radius):
    """判断导弹到顶点的视线是否被烟幕遮挡"""
    vec_AC = smoke_center - missile_pos
    vec_AB = vertex_pos - missile_pos
    ab_sq_mag = np.dot(vec_AB, vec_AB)
    if ab_sq_mag < 1e-12:
        return False  # 导弹与顶点重合（无效）

    k = np.dot(vec_AC, vec_AB) / ab_sq_mag  # 最近点参数
    if k <= 0.0:
        min_dist = np.linalg.norm(vec_AC)
    elif 0.0 < k < 1.0:
        min_dist = np.linalg.norm(np.cross(vec_AC, vec_AB)) / np.sqrt(ab_sq_mag)
    else:
        min_dist = np.linalg.norm(smoke_center - vertex_pos)
    return min_dist <= smoke_radius + 1e-6


def is_effective_occlusion(t, v, theta, t_drop, t_detonate):
    """判断时间t时烟幕是否有效遮蔽真目标"""
    detonate_t = t_drop + t_detonate
    smoke_start, smoke_end = detonate_t, detonate_t + smoke_valid_time
    if t < smoke_start - 1e-6 or t > smoke_end + 1e-6 or t >= missile_time_to_fake - 1e-6:
        return False  # 时间无效

    missile_pos = missile_real_time_position(missile_M1, t)
    smoke_center = smoke_real_time_center(drone_FY1, v, theta, t_drop, t_detonate, t)
    # 任一顶点被遮挡即有效
    return any(is_segment_sphere_intersect(missile_pos, vertex, smoke_center, smoke_radius)
               for vertex in target_vertices)


# -------------------------- 2. 遗传算法+模拟退火（GA+SA）实现 --------------------------
class GA_SA_Optimizer:
    def __init__(self, pop_size, num_dimensions, bounds, drone_init_pos):
        """初始化GA+SA优化器"""
        self.pop_size = pop_size  # 种群大小
        self.num_dimensions = num_dimensions  # 8维变量
        self.bounds = bounds  # 变量边界
        self.drone_init_pos = drone_init_pos

        # 种群与最优解
        self.population = self.initialize_population()  # 初始种群
        self.best_solution = None  # 全局最优解
        self.best_fitness = -np.inf  # 全局最优适应度
        self.fitness_history = []  # 适应度历史
        self.single_durations = [0.0] * NUM_SMOKE  # 单枚烟幕时长

        # GA参数
        self.crossover_prob = 0.8  # 交叉概率
        self.mutation_prob = 0.2  # 变异概率
        self.elitism_ratio = 0.1  # 精英保留比例

        # SA参数
        self.init_temp = 100.0  # 初始温度
        self.cooling_rate = 0.99  # 降温系数
        self.min_temp = 1e-3  # 最低温度

        # 计算参数
        self.time_step = 0.01  # 时间步长（优化后使用0.1秒，平衡精度和效率）
        
        # 日志记录
        self.log_data = []  # 存储每一代的详细日志数据

    def initialize_population(self):
        """初始化满足约束的种群（8维：v, theta, t_drop1-3, t_detonate1-3）"""
        population = []
        while len(population) < self.pop_size:
            candidate = [
                # 无人机全局参数
                np.random.uniform(self.bounds[0][0], self.bounds[0][1]),  # v
                np.random.uniform(self.bounds[1][0], self.bounds[1][1]),  # theta
                # 投放延迟（确保间隔≥1s）
                np.random.uniform(0, missile_time_to_fake),  # t_drop1（放宽限制）
                np.random.uniform(0, missile_time_to_fake),  # t_drop2
                np.random.uniform(0, missile_time_to_fake),  # t_drop3
                # 起爆延迟（确保起爆时间<67s）
                np.random.uniform(0.5, 15),  # t_detonate1
                np.random.uniform(0.5, 15),  # t_detonate2
                np.random.uniform(0.5, 15)  # t_detonate3
            ]
            # 修复投放间隔约束
            t_drops = sorted(candidate[2:5])
            for i in range(1, 3):
                if t_drops[i] - t_drops[i - 1] < 1.0:
                    t_drops[i] = t_drops[i - 1] + 1.0  # 确定性地加1秒
            candidate[2:5] = t_drops
            # 修复起爆时间约束
            for i in range(3):
                max_det = missile_time_to_fake - candidate[2 + i] - 0.1
                candidate[5 + i] = np.clip(candidate[5 + i], 0.5, max_det)
            # 检查约束
            if self.check_constraints(candidate):
                population.append(candidate)
        return np.array(population)

    def check_constraints(self, solution):
        """检查解是否满足所有约束"""
        v, theta = solution[0], solution[1]
        t_drops = solution[2:5]
        t_detonates = solution[5:8]

        # 速度约束
        if not (drone_speed_range[0] <= v <= drone_speed_range[1]):
            return False
        # 方向角约束
        if not (0 <= theta <= 2 * math.pi):
            return False
        # 投放间隔约束
        if t_drops[1] - t_drops[0] < 1.0 or t_drops[2] - t_drops[1] < 1.0:
            return False
        # 起爆时间约束
        for i in range(3):
            if t_drops[i] + t_detonates[i] >= missile_time_to_fake - 1e-6 or t_detonates[i] <= 0:
                return False
        return True

    def repair_solution(self, solution):
        """修复违反约束的解（确定性修复）"""
        v, theta = solution[0], solution[1]
        t_drops = solution[2:5]
        t_detonates = solution[5:8]

        # 修复速度
        solution[0] = np.clip(v, drone_speed_range[0], drone_speed_range[1])
        # 修复方向角（归一化到0~2π）
        solution[1] = theta % (2 * math.pi)
        # 修复投放间隔
        t_drops_sorted = sorted(t_drops)
        for i in range(1, 3):
            if t_drops_sorted[i] - t_drops_sorted[i - 1] < 1.0:
                t_drops_sorted[i] = t_drops_sorted[i - 1] + 1.0  # 确定性地加1秒
        solution[2:5] = [max(t, 0.1) for t in t_drops_sorted]
        # 修复起爆延迟
        for i in range(3):
            max_det = missile_time_to_fake - solution[2 + i] - 0.1
            solution[5 + i] = np.clip(t_detonates[i], 0.5, max_det)
        return solution

    def fitness_function(self, solution):
        """适应度函数：计算联合有效遮蔽时长（优化计算效率）"""
        if not self.check_constraints(solution):
            return 0.0

        # 解析参数
        v = solution[0]
        theta = solution[1]
        t_drops = solution[2:5]
        t_detonates = solution[5:8]
        params = [(v, theta, t_drops[i], t_detonates[i]) for i in range(NUM_SMOKE)]

        # 计算单枚烟幕有效时长
        single_durations = []
        detonate_times = []
        smoke_intervals = []  # 存储每枚烟幕的有效时间区间
        for i in range(NUM_SMOKE):
            v_i, theta_i, t_drop_i, t_detonate_i = params[i]
            detonate_t = t_drop_i + t_detonate_i
            detonate_times.append(detonate_t)
            smoke_start = detonate_t
            smoke_end = min(detonate_t + smoke_valid_time, missile_time_to_fake)

            if smoke_start >= smoke_end:
                single_durations.append(0.0)
                smoke_intervals.append([])
                continue

            # 逐时间步计算
            total = 0.0
            intervals = []  # 记录有效区间
            prev_effective = False
            start_t = 0.0
            t = smoke_start
            while t <= smoke_end + 1e-6:
                current_effective = is_effective_occlusion(t, v_i, theta_i, t_drop_i, t_detonate_i)
                if current_effective and not prev_effective:
                    start_t = t
                elif not current_effective and prev_effective:
                    total += t - start_t
                    intervals.append((start_t, t))
                prev_effective = current_effective
                t += self.time_step
            if prev_effective:
                total += smoke_end - start_t
                intervals.append((start_t, smoke_end))
            single_durations.append(round(total, 3))
            smoke_intervals.append(intervals)

        self.single_durations = single_durations

        # 计算联合遮蔽时长（合并所有烟幕的有效区间）
        all_intervals = []
        for intervals in smoke_intervals:
            all_intervals.extend(intervals)

        if not all_intervals:
            return 0.0

        # 合并重叠区间
        all_intervals.sort(key=lambda x: x[0])
        merged = [all_intervals[0]]
        for current in all_intervals[1:]:
            last = merged[-1]
            if current[0] <= last[1]:
                merged[-1] = (last[0], max(last[1], current[1]))
            else:
                merged.append(current)

        total_union = sum(end - start for start, end in merged)
        return round(total_union, 3)

    def select_parents(self, fitness):
        """轮盘赌选择父母"""
        fitness = np.clip(fitness, 1e-6, None)  # 避免适应度为0
        prob = fitness / np.sum(fitness)
        parents_idx = np.random.choice(self.pop_size, size=2, p=prob, replace=False)
        return self.population[parents_idx[0]], self.population[parents_idx[1]]

    def crossover(self, parent1, parent2):
        """单点交叉生成子代"""
        if np.random.random() > self.crossover_prob:
            return deepcopy(parent1)  # 不交叉则直接返回父代

        point = np.random.randint(1, self.num_dimensions - 1)  # 随机交叉点
        child = np.concatenate([parent1[:point], parent2[point:]])
        return self.repair_solution(child)  # 修复约束

    def mutate(self, individual):
        """高斯变异（增加多样性）"""
        mutated = deepcopy(individual)
        for i in range(self.num_dimensions):
            if np.random.random() < self.mutation_prob:
                # 按维度进行高斯扰动（标准差为边界范围的10%）
                low, high = self.bounds[i]
                mutated[i] += np.random.normal(0, (high - low) * 0.1)
        return self.repair_solution(mutated)  # 修复约束

    def simulated_annealing(self, solution, current_temp):
        """模拟退火局部优化"""
        current_fitness = self.fitness_function(solution)
        # 随机扰动生成邻域解
        neighbor = self.mutate(solution)
        neighbor_fitness = self.fitness_function(neighbor)

        # 计算能量差（适应度差）
        delta_e = neighbor_fitness - current_fitness
        # 接受更优解，或按概率接受较差解
        if delta_e > 0 or np.random.random() < np.exp(delta_e / current_temp):
            return neighbor, neighbor_fitness
        return solution, current_fitness

    def log_generation_data(self, generation, fitness, single_durations_list):
        """记录当前代的详细数据到日志"""
        for i in range(self.pop_size):
            individual = self.population[i]
            fit = fitness[i]
            single_durs = single_durations_list[i] if single_durations_list else [0, 0, 0]
            
            v = individual[0]
            theta = individual[1]
            t_drops = individual[2:5]
            t_detonates = individual[5:8]
            theta_deg = (theta * 180 / math.pi) % 360
            
            for j in range(NUM_SMOKE):
                t_drop = t_drops[j]
                t_detonate = t_detonates[j]
                detonate_t = t_drop + t_detonate
                
                # 投放点
                drop_x = self.drone_init_pos[0] + v * math.cos(theta) * t_drop
                drop_y = self.drone_init_pos[1] + v * math.sin(theta) * t_drop
                drop_z = self.drone_init_pos[2]
                
                # 起爆点
                explosion_point = get_smoke_explosion_center(
                    self.drone_init_pos, v, theta, t_drop, t_detonate
                )
                
                # 添加到日志
                self.log_data.append({
                    '迭代次数': generation,
                    '烟幕弹编号': j + 1,
                    '无人机速度(m/s)': round(v, 4),
                    '飞行方向角(°)': round(theta_deg, 4),
                    '投放延迟(s)': round(t_drop, 4),
                    '起爆延迟(s)': round(t_detonate, 4),
                    '起爆时间(s)': round(detonate_t, 4),
                    '投放点X(m)': round(drop_x, 4),
                    '投放点Y(m)': round(drop_y, 4),
                    '投放点Z(m)': round(drop_z, 4),
                    '起爆点X(m)': round(explosion_point[0], 4),
                    '起爆点Y(m)': round(explosion_point[1], 4),
                    '起爆点Z(m)': round(explosion_point[2], 4),
                    '单枚有效遮蔽时长(s)': round(single_durs[j], 4) if j < len(single_durs) else 0,
                    '联合有效遮蔽时长(s)': round(fit, 4)
                })

    def optimize(self, max_generations):
        """执行GA+SA优化（优化SA应用，只对精英个体进行）"""
        print(f"\n开始GA+SA优化（种群大小：{self.pop_size}，迭代次数：{max_generations}）")
        start_time = time.time()
        current_temp = self.init_temp  # 初始化SA温度

        # 初始评估
        fitness = np.array([self.fitness_function(ind) for ind in self.population])
        self.best_idx = np.argmax(fitness)
        self.best_solution = self.population[self.best_idx].copy()
        self.best_fitness = fitness[self.best_idx]
        self.fitness_history.append(self.best_fitness)
        
        # 记录初始代数据
        self.log_generation_data(0, fitness, None)
        print(f"初始代最优适应度：{self.best_fitness:.3f}s")

        # 主优化循环
        for gen in range(max_generations):
            # 1. 精英保留（保留最优个体）
            num_elite = int(self.elitism_ratio * self.pop_size)
            elite_indices = np.argsort(fitness)[-num_elite:]
            new_population = [self.population[i].copy() for i in elite_indices]

            # 2. 生成新种群（选择+交叉+变异）
            while len(new_population) < self.pop_size:
                parent1, parent2 = self.select_parents(fitness)
                child = self.crossover(parent1, parent2)
                child = self.mutate(child)
                new_population.append(child)

            # 3. 更新种群和适应度
            self.population = np.array(new_population)
            fitness = np.array([self.fitness_function(ind) for ind in self.population])

            # 4. 模拟退火局部优化：只对精英个体进行
            for idx in elite_indices:
                sa_ind, sa_fit = self.simulated_annealing(self.population[idx], current_temp)
                self.population[idx] = sa_ind
                fitness[idx] = sa_fit

            # 5. 更新全局最优
            current_best_idx = np.argmax(fitness)
            current_best_fitness = fitness[current_best_idx]
            if current_best_fitness > self.best_fitness:
                self.best_solution = self.population[current_best_idx].copy()
                self.best_fitness = current_best_fitness

            # 6. 记录当前代的单枚遮蔽时长
            single_durations_list = []
            for ind in self.population:
                self.fitness_function(ind)  # 计算适应度并更新single_durations
                single_durations_list.append(self.single_durations.copy())
            
            # 7. 记录当前代数据
            self.log_generation_data(gen + 1, fitness, single_durations_list)

            # 8. 降温（SA温度更新）
            current_temp = max(current_temp * self.cooling_rate, self.min_temp)

            # 9. 记录历史并输出进度
            self.fitness_history.append(self.best_fitness)
            if (gen + 1) % 10 == 0:
                print(f"迭代{gen + 1:3d}代：最优适应度 = {self.best_fitness:.3f}s，当前温度 = {current_temp:.3f}")

        # 优化结束
        end_time = time.time()
        print(f"\n优化完成！总耗时：{end_time - start_time:.2f}s")
        print(f"最终全局最优联合遮蔽时长：{self.best_fitness:.3f}s")
        return self.best_solution, self.best_fitness

    def save_results(self, filename="result_ga_sa.xlsx"):
        """保存最优策略到Excel"""
        if self.best_solution is None:
            print("请先执行优化！")
            return None

        v_opt = self.best_solution[0]
        theta_opt = self.best_solution[1]
        t_drops_opt = self.best_solution[2:5]
        t_detonates_opt = self.best_solution[5:8]
        theta_deg_opt = (theta_opt * 180 / math.pi) % 360

        results = []
        for i in range(NUM_SMOKE):
            t_drop_i = t_drops_opt[i]
            t_detonate_i = t_detonates_opt[i]
            detonate_t_i = t_drop_i + t_detonate_i
            # 投放点
            drop_x = self.drone_init_pos[0] + v_opt * math.cos(theta_opt) * t_drop_i
            drop_y = self.drone_init_pos[1] + v_opt * math.sin(theta_opt) * t_drop_i
            # 起爆点
            explosion_point = get_smoke_explosion_center(
                self.drone_init_pos, v_opt, theta_opt, t_drop_i, t_detonate_i
            )
            results.append({
                '烟幕弹编号': i + 1,
                '无人机速度(m/s)': round(v_opt, 2),
                '飞行方向角(°)': round(theta_deg_opt, 2),
                '投放延迟(s)': round(t_drop_i, 2),
                '起爆延迟(s)': round(t_detonate_i, 2),
                '起爆时间(s)': round(detonate_t_i, 2),
                '投放点X(m)': round(drop_x, 2),
                '投放点Y(m)': round(drop_y, 2),
                '投放点Z(m)': round(self.drone_init_pos[2], 2),
                '起爆点X(m)': round(explosion_point[0], 2),
                '起爆点Y(m)': round(explosion_point[1], 2),
                '起爆点Z(m)': round(explosion_point[2], 2),
                '单枚有效遮蔽时长(s)': round(self.single_durations[i], 2),
                '联合有效遮蔽时长(s)': round(self.best_fitness, 2)
            })

        df = pd.DataFrame(results)
        df.to_excel(filename, index=False, engine='openpyxl')
        print(f"\n结果已保存到：{filename}")
        return df

    def save_log(self, filename="log.xlsx"):
        """保存优化过程日志到Excel"""
        if not self.log_data:
            print("没有日志数据可保存！")
            return None
        
        log_df = pd.DataFrame(self.log_data)
        log_df.to_excel(filename, index=False, engine='openpyxl')
        print(f"优化日志已保存到：{filename}")
        return log_df


# -------------------------- 3. 主程序 --------------------------
if __name__ == "__main__":
    # 优化参数设置
    NUM_DIMENSIONS = 8  # 8维变量：v, theta, t_drop1-3, t_detonate1-3
    POP_SIZE = 100  # 种群大小
    MAX_GENERATIONS = 250  # 迭代次数

    # 变量边界
    bounds = [
        (70, 140),  # 0: 无人机速度v
        (0, 2 * math.pi),  # 1: 方向角theta
        (0, missile_time_to_fake),  # 2: t_drop1
        (0, missile_time_to_fake),  # 3: t_drop2
        (0, missile_time_to_fake),  # 4: t_drop3
        (0, 10),  # 5: t_detonate1
        (0, 10),  # 6: t_detonate2
        (0, 10)  # 7: t_detonate3
    ]

    # 初始化优化器并执行优化
    optimizer = GA_SA_Optimizer(
        pop_size=POP_SIZE,
        num_dimensions=NUM_DIMENSIONS,
        bounds=bounds,
        drone_init_pos=drone_FY1
    )
    best_solution, best_fitness = optimizer.optimize(max_generations=MAX_GENERATIONS)

    # 保存结果
    optimizer.save_results()
    optimizer.save_log()
    
    print("\n" + "=" * 50)
    print("GA+SA优化的最优烟幕弹投放策略")
    print("=" * 50)
    v_opt = best_solution[0]
    theta_opt = best_solution[1]
    t_drops_opt = best_solution[2:5]
    t_detonates_opt = best_solution[5:8]
    theta_deg_opt = (theta_opt * 180 / math.pi) % 360

    print(f"无人机参数：速度={v_opt:.2f}m/s，方向角={theta_deg_opt:.2f}°")
    print(f"导弹到达假目标时间：{missile_time_to_fake:.3f}s")
    print("\n烟幕弹详情：")
    for i in range(NUM_SMOKE):
        detonate_t = t_drops_opt[i] + t_detonates_opt[i]
        print(f"\n第{i + 1}枚：")
        print(f"  投放延迟：{t_drops_opt[i]:.2f}s，起爆延迟：{t_detonates_opt[i]:.2f}s")
        print(f"  起爆时间：{detonate_t:.2f}s，单枚遮蔽时长：{optimizer.single_durations[i]:.2f}s")
    print(f"\n联合有效遮蔽总时长：{best_fitness:.2f}s")
    print("=" * 50)